#include "filesystem/be_filesystem.h"
#include "car.h"
// #include "src/utils/bullet/BulletDynamics/Vehicle/btWheelInfo.h"

Car::Car(unsigned int mplayerid, btTransform& transform, boost::shared_ptr<btDynamicsWorld> ownerWorld)
 :	Entity( CAR, ownerWorld ),
	m_throttlesum(0.f),
	m_brakingsum(0.f),
	m_lockaccel(false),
// 	m_forw(false),
// 	m_back(false),
	m_throttle(0.0f),
	m_left(false),
	m_right(false),
// 	m_handbrake(false),
	m_braking(0.0f),
	m_drawChassis(true)
{
	settings = Settings::Instance();
	playerid = mplayerid;

	steeringIncrement = 0.002 * settings->getCVar("carsetting_steeringincrement");
	steeringClamp = 0.001f * settings->getCVar("carsetting_steeringclamp");
	steeringExponentiality = settings->getCVar("carsetting_exponentialsteering");

	setting_joystick = settings->getCVarPtr("joystick");
	
	carDef = ScCarDefManager::instance().getCar("default");

	const ScWheelDef* const wheelDef = ScCarDefManager::instance().getWheel("default");
	assert(wheelDef);

	m_wheelModel = BeModelSystem::Instance()->load(wheelDef->getObjPath(), btVector3(1,1,1));
	m_carModel = BeModelSystem::Instance()->load(carDef->getChassis().getObjPath(), btVector3(1,1,1));

	// ENGINE AND BRAKE VARIABLES
		gEngineForce = 0.0f;
		gBreakingForce = 0.0f;
		gLinVehicleSteering = 0.0f;
		gVehicleSteering = 0.0f;
		maxEngineForce = carDef->getEngine().getForce();	//this should be engine/velocity dependent
		maxBreakingForce = 1.0f;
		defaultBreakingForce = carDef->getBrakes().getForce();
	
	// VEHICLE AND CHASSIS SETUP
		btVector3 localInertia = btVector3( 0.0f, 0.0f, 0.0f );
		float lin_damping = 0.05f;
		float ang_damping = 0.5f;
	
		btTransform carupperoffset;
		carupperoffset.setIdentity();
		carupperoffset.setOrigin( 
			btVector3
			( 
				0,
				2 * carDef->getChassis().getHalfDimensions().y,
				0
			)
		);

		body.addBodyPart_Rigid_Box(
			btVector3( carDef->getChassis().getHalfDimensions().x, carDef->getChassis().getHalfDimensions().y, carDef->getChassis().getHalfDimensions().z ),
			carDef->getChassis().getMass(),
			localInertia,
			transform,
			BeColor( 1.0f, 0.0f, 0.0f, 1.0f ),
			lin_damping,
			ang_damping
		);
		
		m_carChassis = static_cast<BodypartRigid*>(body.m_bodyparts[0].get())->m_body;
		m_carChassis->setActivationState(DISABLE_DEACTIVATION);


		m_vehicleRayCaster = boost::shared_ptr<btDefaultVehicleRaycaster>(new btDefaultVehicleRaycaster(ownerWorld.get()));
		m_vehicle = boost::shared_ptr<bRaycastVehicle>(new bRaycastVehicle(m_tuning,m_carChassis.get(),m_vehicleRayCaster.get()));
		m_vehicle->setCoordinateSystem(0,1,2); // rightIndex,upIndex,forwardIndex

		ownerWorld->addVehicle(m_vehicle.get());

	// WHEELS AND SUSPENSION SETUP
	
		for ( unsigned int i=0; i < carDef->getChassis().getWheelPairAttachments().size(); i++ )
		{
			createWheelPair( i, carDef->getChassis() ); //carDef->getChassis().getWheelPairAttachments()[
		}

// 		btTransform centerofmass;
// 		centerofmass.setIdentity();
// 		centerofmass.setOrigin( btVector3( -0.0f, -5.00f, -1000.0f ) );
// 		m_carChassis->setCenterOfMassTransform(transform*centerofmass);

		m_carChassis->setLinearVelocity(btVector3(0.f,0.f,0.f));
		m_carChassis->setAngularVelocity(btVector3(0.f,0.f,0.f));
		m_carChassis->setGravity(btVector3(0.f,-9.80665f,0.f));
	// 	m_carChassis->setFriction(0.1f);
// 		m_carChassis->setRestitution(1000.f);
		ownerWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),ownerWorld->getDispatcher());
		if (m_vehicle)
		{
			m_vehicle->resetSuspension();
			for (int i=0;i<m_vehicle->getNumWheels();i++)
			{
				//synchronize the wheels with the (interpolated) chassis worldtransform
				m_vehicle->updateWheelTransform(i,true);
			}
		}

	// CALCULATE BRAKING AND THROTTLE HELPER SUMS
		for ( unsigned int i = 0; i < carDef->getChassis().getWheelPairAttachments().size(); i++ )
		{
			m_brakingsum += carDef->getChassis().getWheelPairAttachments()[i]->getBrakeBias();
			m_throttlesum += carDef->getChassis().getWheelPairAttachments()[i]->getEngineBias();
		}
}

void Car::createWheelPair( unsigned int wheelpairindex, const ScChassisDef& chassisattachment )
{
	const ScWheelPairAttachment* wheelpairattachment = chassisattachment.getWheelPairAttachments()[wheelpairindex];

	// WHEEL LEFT
		const ScWheelDef* wheel_left = wheelpairattachment->getWheel( ScWheelPairAttachment::eIdLeft );
		const ScSuspensionDef* suspension_left = wheelpairattachment->getSuspension( ScWheelPairAttachment::eIdLeft );

		btVector3 wheelDirectionCS0(0,-1,0);
		btVector3 wheelAxleCS(-1,0,0);
		float connectionHeight = -chassisattachment.getHalfDimensions().y + wheel_left->getRadius();
		bool isFrontWheel=true; // need to test effect

		btVector3 connectionPointCS0;
		connectionPointCS0 = btVector3(
			-wheelpairattachment->getAxleLength(),
// 			-chassisattachment.getHalfDimensions().x - wheel_left->getWidth(),
			connectionHeight,
// 			-chassisattachment.getHalfDimensions().z + wheel_left->getRadius()
 			-wheelpairattachment->getOffset()
		);
		btWheelInfo& wheel = m_vehicle->addWheel(
			connectionPointCS0,
			wheelDirectionCS0,
			wheelAxleCS,
			suspension_left->getLength(),
			wheel_left->getRadius(),
			m_tuning,
			isFrontWheel
		);

		wheel.m_suspensionStiffness = suspension_left->getStiffness();
		wheel.m_wheelsDampingRelaxation = suspension_left->getDamping();
		wheel.m_wheelsDampingCompression = suspension_left->getCompression();
		wheel.m_rollInfluence = suspension_left->getRollInfluence();
		wheel.m_maxSuspensionForce = suspension_left->getMaximumForce();
		wheel.m_frictionSlip = wheel_left->getFriction();

		m_vehicle->interpolateInfo.push_back( interpolateNormalInfo() );
		drawwheel_info.push_back(  DrawWheel_Info() );

	// WHEEL RIGHT
		const ScWheelDef* wheel_right = wheelpairattachment->getWheel( ScWheelPairAttachment::eIdRight );
		const ScSuspensionDef* suspension_right = wheelpairattachment->getSuspension( ScWheelPairAttachment::eIdRight );

		connectionHeight = -chassisattachment.getHalfDimensions().y + wheel_right->getRadius();
		isFrontWheel=true; // need to test effect

// 		btVector3 connectionPointCS0;
		connectionPointCS0 = btVector3(
			wheelpairattachment->getAxleLength(),
// 			-chassisattachment.getHalfDimensions().x - wheel_right->getWidth(),
			connectionHeight,
// 			-chassisattachment.getHalfDimensions().z + wheel_right->getRadius()
 			-wheelpairattachment->getOffset()
		);
		
		btWheelInfo& wheel2 = m_vehicle->addWheel(
			connectionPointCS0,
			wheelDirectionCS0,
			wheelAxleCS,
			suspension_right->getLength(),
			wheel_right->getRadius(),
			m_tuning,
			isFrontWheel
		);

		wheel2.m_suspensionStiffness = suspension_right->getStiffness();
		wheel2.m_wheelsDampingRelaxation = suspension_right->getDamping();
		wheel2.m_wheelsDampingCompression = suspension_right->getCompression();
		wheel2.m_rollInfluence = suspension_right->getRollInfluence();
		wheel2.m_maxSuspensionForce = suspension_right->getMaximumForce();
		wheel2.m_frictionSlip = wheel_right->getFriction();

		m_vehicle->interpolateInfo.push_back( interpolateNormalInfo() );
		drawwheel_info.push_back(  DrawWheel_Info() );
}

// void Car::createWheel( const ScWheelAttachment& attachment )
// {
// }

void Car::setExpSteering()
{
	if ( gLinVehicleSteering > 1.0f)
		gLinVehicleSteering = 1.0f;
	if ( gLinVehicleSteering < -1.0f)
		gLinVehicleSteering = -1.0f;

	float valcent = fabs(0.001f * gLinVehicleSteering);
	float exp = steeringExponentiality / 10000;
	float maxval = 1.0f;
	float maxvalcent = (float)maxval/1000;
	float middle = valcent * pow(exp+1, valcent);
	float max = maxvalcent * pow(exp+1, maxvalcent);

	// somewhat optimized
// 	float valcent = fabs(0.001f * gLinVehicleSteering);
// 	float exp = steeringExponentiality / 10000;
// 	float middle = valcent * pow(exp+1, valcent);
// 	float max = 32.767f * pow(exp+1, 32.767f);

	if ( gLinVehicleSteering < 0 )
		gVehicleSteering = -middle/max * steeringClamp;
	else
		gVehicleSteering = middle/max * steeringClamp;

}

void Car::applyForcesClient( float reltime, Inputtype input )
{
	if ( m_left )
	{
		gLinVehicleSteering -= steeringIncrement*reltime;
		setExpSteering();
	}
	if ( m_right )
	{
		gLinVehicleSteering += steeringIncrement*reltime;
		setExpSteering();
	}
	
	// wheel feedback
	if ( !*setting_joystick )
	{
// 		if (  !m_left && !m_right && m_vehicle->forwardWheelSumSide != 0 )
// 		{
			if ( (!m_right && !m_left) || (m_left && m_vehicle->forwardWheelSumSide < 0) || (m_right && m_vehicle->forwardWheelSumSide > 0) )
			{
				float maximumx = 100; // bout max m_vehicle->forwardWheelSumSide
				float x = maximumx/2 + (m_vehicle->forwardWheelSumSide);
				float nx = 2.0*(x-maximumx/2.0)/maximumx;

				gLinVehicleSteering += nx * reltime; // * fabs(nx)

				setExpSteering();
			}
// 		}
	}
}

void Car::applyForces( float reltime, Inputtype input )
{
	// BREAKING & THROTTLE
		for ( unsigned int i = 0; i < carDef->getChassis().getWheelPairAttachments().size(); i++ )
		{
			// BREAKING
			float relativebreakingforce = (gBreakingForce / m_brakingsum) * carDef->getChassis().getWheelPairAttachments()[i]->getBrakeBias();

			m_vehicle->setBrake(relativebreakingforce,(i*2));
			m_vehicle->setBrake(relativebreakingforce,(i*2)+1);

			// THROTTLE
			if ( !m_lockaccel )
			{
				float relativethrottleforce = (gEngineForce / m_throttlesum) * carDef->getChassis().getWheelPairAttachments()[i]->getEngineBias();

				m_vehicle->applyEngineForce(relativethrottleforce,(i*2));
				m_vehicle->applyEngineForce(relativethrottleforce,(i*2)+1);
			}
		}

	// STEERING
		for ( unsigned int i = 0; i < carDef->getChassis().getWheelPairAttachments().size(); i++ )
		{
			float influence = carDef->getChassis().getWheelPairAttachments()[i]->getSteeringInfluence();

			if ( influence != 0 && influence <= 100 && influence >= -100 )
			{
				float factor = influence / 100;
				m_vehicle->setSteeringValue(gVehicleSteering*factor,(i*2));
				m_vehicle->setSteeringValue(gVehicleSteering*factor,(i*2)+1);
			}
		}
}

void Car::updatePosition()
{
	for ( unsigned int b = 0; b < body.m_bodyparts.size(); b++ )
		body.m_bodyparts[b]->updatePosition();
	
	btTransform t;
	t.setIdentity(); 
	t.getBasis().setEulerZYX(0, SIMD_PI, 0);
	
	for ( unsigned int i = 0; i < carDef->getChassis().getWheelPairAttachments().size(); i++ )
	{
		for ( unsigned int j = 0; j < 2; j++ )
		{
			unsigned int index = (2*i)+j;
			m_vehicle->updateWheelTransform( index ,true);
			m_vehicle->getWheelInfo( index ).m_worldTransform.getOpenGLMatrix( drawwheel_info[index].drawwheelbuffers );
// 			for ( unsigned int k = 0; k < 16; k++ )
// 			{
// 				drawwheel_info[index].drawwheelbuffers[k] = drawwheel_info[index].wheelbuffers[k];
// 			}
		}
		// flip left wheels, we only need to flip the right ones, index 2*i
		unsigned int index = 2*i;
		btTransform wheeltransform;
		wheeltransform.setFromOpenGLMatrix(drawwheel_info[index].drawwheelbuffers);
		wheeltransform *= t;
		wheeltransform.getOpenGLMatrix(drawwheel_info[index].drawwheelbuffers);
	}
	
/*	m_vehicle->updateWheelTransform(0,true);
	m_vehicle->getWheelInfo(0).m_worldTransform.getOpenGLMatrix(wh1);
// 	wh1basis = m_vehicle->getWheelInfo(0).m_worldTransform.getBasis();
	m_vehicle->updateWheelTransform(1,true);
	m_vehicle->getWheelInfo(1).m_worldTransform.getOpenGLMatrix(wh2);
// 	wh2basis = m_vehicle->getWheelInfo(1).m_worldTransform.getBasis();
	m_vehicle->updateWheelTransform(2,true);
	m_vehicle->getWheelInfo(2).m_worldTransform.getOpenGLMatrix(wh3);
// 	wh3basis = m_vehicle->getWheelInfo(2).m_worldTransform.getBasis();
	m_vehicle->updateWheelTransform(3,true);
	m_vehicle->getWheelInfo(3).m_worldTransform.getOpenGLMatrix(wh4);
// 	wh4basis = m_vehicle->getWheelInfo(3).m_worldTransform.getBasis();*/
	
}

void Car::draw()
{
	// offset drawing model downwards
// 	btTransform tpos;
// 	tpos.setIdentity(); 
// 	tpos.setOrigin(btVector3(0.f,-0.066667f,0.f));
// 	transform*=tpos;
	if(m_drawChassis)
	{
		// flip drawing model 180
		btTransform t;
		t.setIdentity(); 
		t.getBasis().setEulerZYX(0, SIMD_PI, 0);

		btTransform transform;
		transform.setFromOpenGLMatrix(body.m_bodyparts[0]->drawposition);
		transform *= t;

		float m[16];
		transform.getOpenGLMatrix(m);
		m_carModel->draw(m);
	}
	for ( unsigned int i = 0; i < carDef->getChassis().getWheelPairAttachments().size(); i++ )
	{
		for ( unsigned int j = 0; j < 2; j++ )
		{
			m_wheelModel->draw( drawwheel_info[ (2*i)+j ].drawwheelbuffers );
		}
	}

	return;
}

Car::~Car()
{
	cerr << "exiting car" << endl;
	body.m_ownerWorld->removeVehicle(m_vehicle.get());
}
